#include<ros/ros.h>
#include<sensor_msgs/PointCloud2.h>
#include<geometry_msgs/PoseArray.h>
#include<geometry_msgs/Point.h>
#include<pcl_conversions/pcl_conversions.h>
#include<pcl/point_cloud.h>
#include<pcl/conversions.h> //用于pointcloud2和pointcloud转换
#include<pcl/filters/passthrough.h>
#include<vector>
#include<fstream> 
#include<sstream>
#include<string>
#include<geometry_msgs/PointStamped.h>
#include "../include/dynamic_obstacle/contiobsmap.h"

ros::Publisher obs_pub;
ros::Subscriber point_cloud_sub;
std::vector<std::vector<float>> global_obs;
std::vector<std::vector<float>> static_obs;

//读取离线静态障碍数据
bool loadobstacledata(const std::string& filename,std::vector<std::vector<float>> &static_obs)
{
    std::ifstream file(filename);
    if(!file.is_open())
    {
        ROS_ERROR("failed to open the static_obs file: %s",filename.c_str());
    }
    float x,y;
    while(file>>x>>y)
    {
        static_obs.push_back({x,y});
    }
    file.close();
    return true;
}
// 定义一个回调函数来处理接收到的 PointCloud2 消息
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*msg, *cloud);
    global_obs.clear();
    // 遍历点云数据中的所有点,添加到全局变量global_obs中
    for (const auto& point : cloud->points)
    {
        std::vector<float> obstacle = {point.x, point.y};
        global_obs.push_back(obstacle); 
    }
    //将离线障碍数据添加到obs里面
    for(const auto& obs_temp : static_obs)
    {
        global_obs.push_back(obs_temp);
    }



    ROS_INFO("Extracted %zu obstacles.", global_obs.size()); 
    // 打印每个障碍物的坐标
    for (const auto point : global_obs)
    {
        ROS_INFO("Obstacle at (x: %f, y: %f)", point[0], point[1]);
    }
}
/*void publish_obstacles()
{
    geometry_msgs::PointStamped msg;
    for(int i = 0; i < global_obs.size(); ++i)
    {
        msg.point.x = global_obs[i][0];
        msg.point.y = global_obs[i][1];
        msg.point.z = 0;
        msg.header.frame_id = "map";  // 设置 frame_id
        obs_pub.publish(msg);
        //ROS_INFO("Published obstacle at (x: %f, y: %f, z: %f).", msg.x, msg.y, msg.z);
    }
}*/
void publish_obstacles()
{
    geometry_msgs::Point msg;
    for(int i = 0; i < global_obs.size(); ++i)
    {
        msg.x = global_obs[i][0];
        msg.y = global_obs[i][1];
        msg.z = 0;
        obs_pub.publish(msg);
        //ROS_INFO("Published obstacle at (x: %f, y: %f, z: %f).", msg.x, msg.y, msg.z);
    }
}
/*void publish_obstacles()
{
    geometry_msgs::PoseArray msg;
    msg.poses.resize(global_obs.size());
    for(int i=0;i<global_obs.size();i++)
    {
        geometry_msgs::Pose pose;
        pose.position.x=global_obs[i][0];
        pose.position.y=global_obs[i][1];
        pose.position.z=0;
        msg.poses[i]=pose;
    }
    msg.header.frame_id = "map"; // 设置 frame_id
    obs_pub.publish(msg);
    ROS_INFO("Published %zu obstacles.", global_obs.size());
}*/
/*void publish_obstacles()
{
    sensor_msgs::PointCloud2 msg;
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
    // 设置点云的宽度、高度和存储格式
    pcl_cloud.width = global_obs.size();
    pcl_cloud.height = 1;
    pcl_cloud.is_dense = false;
    pcl_cloud.points.resize(pcl_cloud.width * pcl_cloud.height);
    // 填充点云数据
    for (size_t i = 0; i < global_obs.size(); ++i)
    {
        pcl_cloud.points[i].x = global_obs[i][0];
        pcl_cloud.points[i].y = global_obs[i][1];
        pcl_cloud.points[i].z = 0.0;
    }
    // 将 pcl 点云转换为 sensor_msgs::PointCloud2 消息
    pcl::toROSMsg(pcl_cloud, msg);
    msg.header.frame_id = "map"; // 设置 frame_id
    obs_pub.publish(msg);
    ROS_INFO("Published %zu points in PointCloud2.", global_obs.size());
}*/
int main(int argc, char** argv)
{
    //读取静态障碍数据加载到static_obs变量中。
    std::string file_name="/home/liuph/ptcmap/src/dynamic_obstacle/src/obstacle_data.txt";
    bool aa=loadobstacledata(file_name,static_obs);
    // 初始化 ROS 节点，节点名称为 "dynamic_obstacle"
    ros::init(argc, argv, "dynamic_obstacle");
    ros::NodeHandle nh;
    //发布合并后的障碍物信息到planningobs0_topic
    obs_pub=nh.advertise<geometry_msgs::Point>("/planningobs0_topic",100);
    //obs_pub=nh.advertise<geometry_msgs::PoseArray>("/planningobs0_topic",100);
    //obs_pub=nh.advertise<sensor_msgs::PointCloud2>("/planningobs0_topic",100);
    // 订阅 "/total_Material_topic" 话题，注册回调函数 pointCloudCallback
    ros::Subscriber sub = nh.subscribe("/total_Material_topic", 10, pointCloudCallback);
    // 设置 ROS 循环的频率为 0.25 Hz（每秒 0.25 次）
    ros::Rate rate(0.25); 

    // 在 ROS 节点运行期间，不断调用回调函数，直到节点关闭
    while (ros::ok())
    {
        ros::spinOnce();  // 调用一次回调函数
        publish_obstacles();  // 发布障碍物坐标
        rate.sleep();  // 按照设定的频率休眠
        vector<vector<float>> obs1={{11,22},{11,13},{11,11},{12,12}};
        int dx=3,dy=3;
        int rsafe=1.71;
        vector<float> zmin={-10,-20};
        vector<float> zmax={10,20};
        vector<int> nz={400,400,50};
        vector<float> Ahmap={10,20};
        vector<vector<float>> pilerange={{-5,-10},{5,10}};
        Gridmap result=contiObsMap_dymap_G(obs1, dx, dy, rsafe, zmin, zmax,nz, Ahmap, pilerange);
            

    }

    return 0;  // 返回 0，程序结束
}
